clc; clear; close all;

%%%%  输入网络的图像大小为268*534 GRAY8PP  %%%%  
%%%%  输入网络的超声信号为rf_data:1578(可变)*64 double, acoustic_impedance  5.57  %%%%
%%%%  输入网络的光声信号为rf_data:1378(可变)*64 double, optical_absorption 0.800  %%%%  

% load('D:\Project\Results\Training_40MHz\carbon_none_bone_acq_1_frame_7_PA_40MHz_rf.mat');
% % 检查 rf_data 的维度
% if size(rf_data, 2) == 64
%     % 创建一个图形窗口
%     figure;
%     
%     % 循环绘制每一维的信号
%     for i = 1:64
%         subplot(8, 8, i); % 将图形窗口分为 8x8 的子图，方便显示 64 个信号
%         plot(rf_data(:, i)); % 绘制第 i 维的信号
%         title(['Signal ', num2str(i)]); % 设置子图标题
%         xlabel('Sample Index'); % 设置 x 轴标签
%         ylabel('Amplitude'); % 设置 y 轴标签
%     end
% else
%     error('rf_data does not have 64 dimensions.');
% end



% Load the data file
load('D:\Project\Data\carbon_with2point5mmbone\carbon_with2point5mmbone\carbon_with1point5mmbone.mat');
file_name = 'carbon_with1point5mmbone';   %%%%  remember to modify

%% Basic Parameter Setup
% These parameters are defined by the experimental setup and are consistent
% for both PA and US imaging.

% General acquisition parameters
params.fs = Receive(1).decimSampleRate * 1e6; % [Hz] Sampling frequency
params.fc = Trans.frequency * 1e6;          % [Hz] Transducer center frequency
params.c = 1500;                            % [m/s] Speed of sound in the medium
% Transducer geometry
params.Nelements = 64;
params.pitch = 0.300e-3;    % [m] Element pitch
params.width = 0.280e-3;    % [m] Element width
params.radius = inf;        % [m] Linear array
% Imaging parameters
params.bandwidth = 90;      % [%] Relative bandwidth
params.fnumber = [];        % Use automatic f-number calculation in DAS

%% Data Separation (PA and US)
% This section separates the interleaved PA and US data from the raw RcvData.

% Define the number of acquisitions per frame for PA and US
ne = 5; % Number of Photoacoustic (PA) acquisitions per frame
na = 5; % Number of Ultrasound (US) acquisitions per frame

% --- Photoacoustic (PA) Data Extraction ---
RcvData_PA = {RcvData{1}(1:Receive(ne).endSample,:,:)};

% Create indices to select only the PA acquisitions
Indices1_PA = repelem((0:(size(RcvData_PA{1},3)-1)).*(ne+na), ne);
Indices2_PA = repmat(1:ne, 1, size(RcvData_PA{1},3));
PAReceiveIndices = Indices1_PA + Indices2_PA;
Receive_PA = Receive(PAReceiveIndices);

% Reorder frames to match acquisition sequence
frame_order = [Resource.RcvBuffer.lastFrame+1:Resource.RcvBuffer.numFrames, 1:Resource.RcvBuffer.lastFrame];

% Read and organize PA data
data_PA = zeros(numel(Receive_PA(1).startSample:Receive_PA(1).endSample), Trans.numelements, ne, numel(frame_order));
n_pa = 1;
frame_idx_pa = 0;
for n_frame_pa = frame_order
    frame_idx_pa = frame_idx_pa + 1;
    for n_tx_pa = 1:ne
        data_PA(:,:,n_tx_pa,frame_idx_pa) = RcvData_PA{1}(Receive_PA(n_pa).startSample:Receive_PA(n_pa).endSample, Trans.Connector, n_frame_pa);
        n_pa = n_pa + 1;
    end
end

% --- Ultrasound (US) Data Extraction ---
RcvData_US = {RcvData{1}(Receive(ne+1).startSample:Receive(ne+na).endSample,:,:)};

% Create indices to select only the US acquisitions
Indices1_US = repelem((0:(size(RcvData_US{1},3)-1)).*(ne+na), na);
Indices2_US = repmat((ne+1):(ne+na), 1, size(RcvData_US{1},3));
USReceiveIndices = Indices1_US + Indices2_US;
Receive_US = Receive(USReceiveIndices);

% Adjust sample indices for the extracted US data block
sampleDisplacement = Receive_US(1).startSample - 1;
for i = 1:numel(Receive_US)
    Receive_US(i).startSample = Receive_US(i).startSample - sampleDisplacement;
    Receive_US(i).endSample = Receive_US(i).endSample - sampleDisplacement;
end

% Read and organize US data
data_US = zeros(numel(Receive_US(1).startSample:Receive_US(1).endSample), Trans.numelements, na, numel(frame_order));
n_us = 1;
frame_idx_us = 0;
for n_frame_us = frame_order
    frame_idx_us = frame_idx_us + 1;
    for n_tx_us = 1:na
        data_US(:,:,n_tx_us,frame_idx_us) = RcvData_US{1}(Receive_US(n_us).startSample:Receive_US(n_us).endSample, Trans.Connector, n_frame_us);
        n_us = n_us + 1;
    end
end




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  按照40MHz采样频率的插值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%% 0. 路径与文件夹
saveDir = 'D:\Project\Results\Training_40MHz';   % 自行修改
if ~exist(saveDir,'dir'), mkdir(saveDir); end

%% 1. 计算旧采样率（原来代码里已经算过，这里直接拿）
fs_old = params.fs;          % 原始采样率，单位 Hz
fs_new = 40e6;               % 目标采样率 40 MHz

%% 2. 对 US 数据做 40 MHz 插值
% 2-1 先把四维 RF 数据展开成 [Ns_old × Nch × Ntx × Nframe]
% data_US 已经在原脚本里生成
Ns_old = size(data_US,1);
Ns_new = round(Ns_old * fs_new / fs_old);   % 插值后的采样点数


% Assume speed of sound c in mm/us (common value for soft tissue in ultrasound)
c = 1.54e3; % mm/us; adjust if needed for your medium

% Default min and max distances in mm
% Formula for time window: t_min = 2 * min_dist / c; t_max = 2 * max_dist / c;
% (factor of 2 accounts for round-trip time in ultrasound echo)
min_dist = 2 * 25e-3; % mm; minimum distance
max_dist = 2 * sqrt(20^2 + 25^2) * 1e-3; % mm; maximum distance, approx 32.02 mm

% Calculate time window in us
t_min = min_dist / c;
t_max = max_dist / c;

% Convert to indices in the new time axis
% t_new_PA goes from 0 to (Ns_new_PA-1)/fs_new with step 1/fs_new
idx_min = floor(t_min * fs_new) + 1; % +1 for 1-based indexing
idx_max = ceil(t_max * fs_new); % no +1 needed after ceil, but ensure within bounds
idxSplit_US = idx_max;           % 前 3/4 与后 1/4 的分界

% 构造新的时间轴
t_old = (0:Ns_old-1)' / fs_old;
t_new = (0:Ns_new-1)' / fs_new;

% 预分配插值后数组
data_US_40MHz = zeros(Ns_new, size(data_US,2), size(data_US,3), size(data_US,4));

% 逐通道、逐发射、逐帧插值（线性插值速度足够，可改 spline）
for f = 1:size(data_US,4)
    for tx = 1:size(data_US,3)
        for ch = 1:size(data_US,2)
            sig = squeeze(data_US(:,ch,tx,f));
            sig_i = interp1(t_old, sig, t_new, 'linear', 0);
            data_US_40MHz(:,ch,tx,f) = sig_i;
        end
    end
end



%% 3. 对 PA 数据做 40 MHz 插值（同上）
Ns_old_PA = size(data_PA,1);
Ns_new_PA = round(Ns_old_PA * fs_new / fs_old);

% Assume speed of sound c in mm/us (common value for soft tissue in ultrasound)
c = 1.54e3; % mm/us; adjust if needed for your medium

% Default min and max distances in mm
% Formula for time window: t_min = 2 * min_dist / c; t_max = 2 * max_dist / c;
% (factor of 2 accounts for round-trip time in ultrasound echo)
min_dist = 25e-3; % mm; minimum distance
max_dist = sqrt(20^2 + 26.5^2) * 1e-3; % mm; maximum distance, approx 32.02 mm

% Calculate time window in us
t_min = min_dist / c;
t_max = max_dist / c;

% Convert to indices in the new time axis
% t_new_PA goes from 0 to (Ns_new_PA-1)/fs_new with step 1/fs_new
idx_min = floor(t_min * fs_new) + 1; % +1 for 1-based indexing
idx_max = ceil(t_max * fs_new); % no +1 needed after ceil, but ensure within bounds

% Clamp indices to valid range
idx_min = max(1, idx_min);
idx_max = min(Ns_new_PA, idx_max);

t_old_PA = (0:Ns_old_PA-1)' / fs_old;
t_new_PA = (0:Ns_new_PA-1)' / fs_new;

data_PA_40MHz = zeros(Ns_new_PA, size(data_PA,2), size(data_PA,3), size(data_PA,4));

for f = 1:size(data_PA,4)
    for tx = 1:size(data_PA,3)
        for ch = 1:size(data_PA,2)
            sig = squeeze(data_PA(:,ch,tx,f));
            sig_i = interp1(t_old_PA, sig, t_new_PA, 'linear', 0);
            data_PA_40MHz(:,ch,tx,f) = sig_i;
        end
    end
end


%% 4. 更新参数结构体
params_40MHz        = params;      % 复制原参数
params_40MHz.fs     = fs_new;      % 换成 40 MHz
params_40MHz.fc     = params.fc;   % 中心频率不变（只是采样率变了）
params_40MHz.c      = params.c;

%% 5. 重新 IQ 解调（用新采样率）
% 这里只取第一帧、第一次发射做演示；如需全帧请做循环
acq_num = 4;   %%%  ≤ 5  %%%%  remember to modify
acq_num_str = num2str(acq_num);
frame_num = 1;  %%%  ≤ 10    %%%%  remember to modify
frame_num_str = num2str(frame_num);
sigUS_full = squeeze(data_US_40MHz(:, :, acq_num, frame_num));
[sigUS_front, sigUS_back] = splitSignal(sigUS_full, idxSplit_US);
iq_US_40MHz = rf2iq(sigUS_full, fs_new, params_40MHz.fc);
iq_US_front = rf2iq(sigUS_front, fs_new, params_40MHz.fc);
iq_US_back = rf2iq(sigUS_back, fs_new, params_40MHz.fc);


sigPA_full = squeeze(data_PA_40MHz(:, :, acq_num, frame_num));
[sigPA_front, sigPA_back] = splitSignal(sigPA_full, idx_max);
iq_PA_40MHz = rf2iq(sigPA_full, fs_new, params_40MHz.fc);
iq_PA_front = rf2iq(sigPA_front, fs_new, params_40MHz.fc);
iq_PA_back = rf2iq(sigPA_back, fs_new, params_40MHz.fc);

% --- Imaging Grid Setup ---
xlen = params.pitch * (params.Nelements - 1) + params.width;
x_imaging_axis = (-xlen/2):1e-4:(xlen/2);
z_imaging_axis = 0:0.1e-3:40e-3;
[xi, zi] = meshgrid(x_imaging_axis, z_imaging_axis);

%% 6. 重新 DAS 波束合成（网格与原脚本相同）
params_US_40MHz = params_40MHz;
bfsig_US_40MHz  = das(iq_US_40MHz, xi, zi, zeros(1,params_40MHz.Nelements), params_US_40MHz);
bfsig_US_front  = das(iq_US_front, xi, zi, zeros(1,params_40MHz.Nelements), params_US_40MHz);
bfsig_US_back  = das(iq_US_back, xi, zi, zeros(1,params_40MHz.Nelements), params_US_40MHz);



params_PA_40MHz         = params_40MHz;
params_PA_40MHz.passive = true;
bfsig_PA_40MHz          = das(iq_PA_40MHz, xi, zi, zeros(1,params_40MHz.Nelements), params_PA_40MHz);
bfsig_PA_front          = das(iq_PA_front, xi, zi, zeros(1,params_40MHz.Nelements), params_PA_40MHz);
bfsig_PA_back          = das(iq_PA_back, xi, zi, zeros(1,params_40MHz.Nelements), params_PA_40MHz);

%% 7. 保存插值后的信号
rf_data = sigUS_full;
% 归一化操作
rf_data = (rf_data - min(rf_data(:))) / (max(rf_data(:)) - min(rf_data(:))); % 归一化到 [0, 1]
rf_data = 2 * rf_data - 1; % 转换到 [-1, 1]

acoustic_impedance = 5.57;
save(fullfile(saveDir, [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_US_40MHz_rf.mat']), 'rf_data', 'acoustic_impedance');
rf_data = sigUS_front;
% 归一化操作
rf_data = (rf_data - min(rf_data(:))) / (max(rf_data(:)) - min(rf_data(:))); % 归一化到 [0, 1]
rf_data = 2 * rf_data - 1; % 转换到 [-1, 1]
acoustic_impedance = 5.57;
save(fullfile(saveDir,[file_name '_acq_' acq_num_str '_frame_' frame_num_str '_US_front_rf.mat']), 'rf_data', 'acoustic_impedance');
rf_data = sigUS_back;
% 归一化操作
rf_data = (rf_data - min(rf_data(:))) / (max(rf_data(:)) - min(rf_data(:))); % 归一化到 [0, 1]
rf_data = 2 * rf_data - 1; % 转换到 [-1, 1]
acoustic_impedance = 5.57;
save(fullfile(saveDir,[file_name '_acq_' acq_num_str '_frame_' frame_num_str '_US_back_rf.mat']), 'rf_data', 'acoustic_impedance');

rf_data = sigPA_full;
% 归一化操作
rf_data = (rf_data - min(rf_data(:))) / (max(rf_data(:)) - min(rf_data(:))); % 归一化到 [0, 1]
rf_data = 2 * rf_data - 1; % 转换到 [-1, 1]
optical_absorption = 0.8;
save(fullfile(saveDir, [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_PA_40MHz_rf.mat']), 'rf_data', 'optical_absorption');
rf_data = sigPA_front;
% 归一化操作
rf_data = (rf_data - min(rf_data(:))) / (max(rf_data(:)) - min(rf_data(:))); % 归一化到 [0, 1]
rf_data = 2 * rf_data - 1; % 转换到 [-1, 1]
optical_absorption = 0.8;
save(fullfile(saveDir,[file_name '_acq_' acq_num_str '_frame_' frame_num_str '_PA_front_rf.mat']), 'rf_data', 'optical_absorption');
rf_data = sigPA_back;
% 归一化操作
rf_data = (rf_data - min(rf_data(:))) / (max(rf_data(:)) - min(rf_data(:))); % 归一化到 [0, 1]
rf_data = 2 * rf_data - 1; % 转换到 [-1, 1]
optical_absorption = 0.8;
save(fullfile(saveDir, [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_PA_back_rf.mat']), 'rf_data', 'optical_absorption');

%% 8. 保存重建图像（PNG）
% 8-1 US 图像
log_compressed_parameter_us = 18;   %%% 40
I_US = bmode(bfsig_US_40MHz, log_compressed_parameter_us);
im_US = imagesc(x_imaging_axis*1000, z_imaging_axis*1000, I_US);
I_US_front = bmode(bfsig_US_front, log_compressed_parameter_us);
im_US_front = imagesc(x_imaging_axis*1000, z_imaging_axis*1000, I_US_front);
I_US_back = bmode(bfsig_US_back, log_compressed_parameter_us);
im_US_back = imagesc(x_imaging_axis*1000, z_imaging_axis*1000, I_US_back);
% colormap(gray); axis image off; 
% title(''); 
% set(gca,'Visible','off');
% saveas(gcf, fullfile(saveDir,'US_image_40MHz.png'));
% close(gcf);
png_name = [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_US_image_40MHz_das.png'];
saveImage(I_US, saveDir, png_name);
png_name = [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_US_image_front_das.png'];
saveImage(I_US_front, saveDir, png_name);
png_name = [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_US_image_back_das.png'];
saveImage(I_US_back, saveDir, png_name);


% 8-2 PA 图像
log_compressed_parameter_pa = 16;
I_PA = bmode(bfsig_PA_40MHz, log_compressed_parameter_pa);
im_PA = imagesc(x_imaging_axis*1000, z_imaging_axis*1000, I_PA);
I_PA_front = bmode(bfsig_PA_front, log_compressed_parameter_pa);
im_PA_front = imagesc(x_imaging_axis*1000, z_imaging_axis*1000, I_PA_front);
I_PA_back = bmode(bfsig_PA_back, log_compressed_parameter_pa);
im_PA_back = imagesc(x_imaging_axis*1000, z_imaging_axis*1000, I_PA_back);
% colormap(hot); axis image off;
% title('');
% set(gca,'Visible','off');
% saveas(gcf, fullfile(saveDir,'PA_image_40MHz.png'));
% close(gcf);

png_name = [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_PA_image_40MHz_das.png'];
saveImage(I_PA, saveDir, png_name);
png_name = [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_PA_image_front_das.png'];
saveImage(I_PA_front, saveDir, png_name);
png_name = [file_name '_acq_' acq_num_str '_frame_' frame_num_str '_PA_image_back_das.png'];
saveImage(I_PA_back, saveDir, png_name);



% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % %%%%  按照10MHz原始采样频率
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % 
% % %% Image Reconstruction
% % % This section performs beamforming and creates the images.
% % 
% % % --- IQ Demodulation ---
% % % Convert RF data to IQ (in-phase and quadrature) data.
% % % The initial samples are removed to exclude laser trigger delays or transmit artifacts.
% % IQ_PA = rf2iq(data_PA(9:end,:,1,1), params.fs, params.fc);
% % IQ_US = rf2iq(data_US(8:end,:,1,1), params.fs, params.fc);
% % 
% % % --- Imaging Grid Setup ---
% % xlen = params.pitch * (params.Nelements - 1) + params.width;
% % x_imaging_axis = (-xlen/2):1e-4:(xlen/2);
% % z_imaging_axis = 0:0.1e-3:40e-3;
% % [xi, zi] = meshgrid(x_imaging_axis, z_imaging_axis);
% % 
% % % --- PA Reconstruction ---
% % params_PA = params;
% % params_PA.passive = true; % PA is a passive modality (receive-only)
% % bfsig_PA = das(IQ_PA, xi, zi, zeros(1, params.Nelements), params_PA);
% % 
% % % --- US Reconstruction ---
% % params_US = params;
% % bfsig_US = das(IQ_US, xi, zi, zeros(1, params.Nelements), params_US);
% % 
% % 
% % %% Visualization
% % % This section displays the reconstructed PA and US images.
% % 
% % % --- Display PA Image ---
% % log_compressed_parameter_pa = 16;
% % figure;
% % IPA = bmode(bfsig_PA, log_compressed_parameter_pa);
% % imagesc(x_imaging_axis * 1000, z_imaging_axis * 1000, IPA);
% % colormap(hot);
% % title('PA Image (DAS)', 'FontSize', 16);
% % xlabel('Lateral Distance (mm)');
% % ylabel('Depth (mm)');
% % colorbar;
% % axis image;
% % 
% % % --- Display US Image ---
% % log_compressed_parameter_us = 40;
% % figure;
% % IUS = bmode(bfsig_US, log_compressed_parameter_us);
% % imagesc(x_imaging_axis * 1000, z_imaging_axis * 1000, IUS);
% % colormap(gray);
% % title('US Image (DAS)', 'FontSize', 16);
% % xlabel('Lateral Distance (mm)');
% % ylabel('Depth (mm)');
% % colorbar;
% % axis image;

% % --- Display Zoomed Images ---
% figure;
% imagesc(x_imaging_axis * 1000, z_imaging_axis(201:end) * 1000, IPA(201:end,:));
% colormap(hot);
% title('Zoomed PA Image', 'FontSize', 16);
% xlabel('Lateral Distance (mm)');
% ylabel('Depth (mm)');
% axis image;
% 
% figure;
% imagesc(x_imaging_axis * 1000, z_imaging_axis(201:end) * 1000, IUS(201:end,:));
% colormap(gray);
% title('Zoomed US Image', 'FontSize', 16);
% xlabel('Lateral Distance (mm)');
% ylabel('Depth (mm)');
% axis image;


%% Profile Analysis
% Compare the signal profile from the PA image with a reference.
load('D:\Project\Data\carbon_with2point5mmbone\carbon_with2point5mmbone\cc.mat'); % Load reference profile data

figure;
plot(cc, 'linewidth', 2);
hold on;
plot(I_PA(343,:), 'linewidth', 2);
title('Effect of Bone on Target Imaging');
legend({'Target without bone', 'Target with bone plate'});
grid on;
xlabel('Lateral Position');
ylabel('Amplitude');
set(gca, 'FontSize', 12);


%% 拆分函数（减少重复代码）
function [sigA, sigB] = splitSignal(sig, idx)
    % sig: [Ns, ...]
    sigA = sig;
    sigA(idx+1:end,:) = 0;          % 前 3/4，后 1/4 填零
    sigB = sig;
    sigB(1:idx,:) = 0;              % 后 1/4，前 3/4 填零
end

%% 通用保存图像函数
function saveImage(I, saveDir, fname)
    % 将重建图像的数据范围缩放到 [0, 255] 以便于保存为图像
    scaled_image = mat2gray(I);
    %% 调整尺寸：目标 [宽 268, 高 534]以便可以输入网络
    scaled_image = imresize(scaled_image, [534 268], 'bilinear');
    % 使用 imwrite 函数保存图像
    image_path = fullfile(saveDir, fname);
    imwrite(uint8(scaled_image * 255), image_path);
%     figure;
%     imagesc(x_imaging_axis * 1000, z_imaging_axis * 1000, scaled_image);
%     colormap(gray);
%     title('US Image (DAS)', 'FontSize', 16);
%     xlabel('Lateral Distance (mm)');
%     ylabel('Depth (mm)');
%     colorbar;
%     axis image;
end